clear all;
load groundTruth.dat;
addpath(genpath('./KalmanAll/'))

dataA = groundTruth;

% The required frame rate is 33 ms per frame. Our data contains observations 
% that are taken at time intervals that are multiples of 100 ms. Thus, we 
% need to interpolate these observations. 

size(dataA)

%--------------------------------------------------------------------------
% Interpolate data
interpolatedTimeStamps = (dataA(1,1):100/3:dataA(size(dataA,1),1));
newDataA = zeros(size(interpolatedTimeStamps',1),3);
newDataA(:,1) = interpolatedTimeStamps';
newDataA(:,2) = (interp1(dataA(:,1)', dataA(:,2)', interpolatedTimeStamps))';
newDataA(:,3) = (interp1(dataA(:,1)', dataA(:,3)', interpolatedTimeStamps))';
%--------------------------------------------------------------------------

%============================= Ignore this bit of code for now  ===========================
% TODO: May want to smoothen inputs before training

% Use values of x-coordinate to calculate acceleration

% Matrices that define the relation between position, velocity
% and acceleration

T = size(newDataA,1); % Total number of frames
deltaT = 100/3; % 33 ms
A = [1 deltaT; 0 1];
B = [deltaT * deltaT/2; deltaT];
C = [1 0];

% This will store the values of acceleration in px/ms^2
u_c = zeros(1,T+1); % a_c(0) to a_c(T)

multiplier = A * [0;0];

for t = 2:T
    u_c(t) = (newDataA(t,2) - (C * multiplier)) / (C * B);
    multiplier = A * (multiplier + B * u_c(t));
end

u_cs = smooth(u_c');

t=1:size(u_c,2);
plot(t,u_cs);
legend('Smooth');
print('graph2.png', '-dpng')

% TODO: Smoothing may happen here instead of above - Judging by the values I got here

%==========================================================================================

% For now, train and test using the groundth truth co-ordinates as training values instead of u_c
y = newDataA(:,2);
y = smooth(y,150);
% Prepare features first

load lecturer.dat;
load saliency.dat;
load boards.dat;
size(lecturer)
size(saliency)
size(y)

% Scale vectors
lecturer = lecturer * 640/2560;
saliency = saliency * 640/1920;

% There are small differences in timestamps in each of these, so just combine corresponding entries
% Train on 30000 frames and test on the rest
% Features: [LecturerX, SaliencyX, SaliencyY, LecturerX 2 secs ago, SaliencyX 2 secs ago, SaliencyY 2 secs ago,
% LecturerX 5 secs ago, SaliencyX 5 secs ago, SaliencyY 5 secs ago, LecturerX 10 secs ago, SaliencyX 10 secs ago, SaliencyY 10 secs ago] 

numFeatures = 12;
numTrainPoints = 30000;
numTestPoints = 20000;
trainFeatures = zeros(numFeatures, numTrainPoints);
trainFeatures(1,:) = lecturer(1:numTrainPoints,2)';
trainFeatures(2,:) = saliency(1:numTrainPoints,2)';
trainFeatures(3,:) = saliency(1:numTrainPoints,3)';

trainFeatures(4,61:numTrainPoints) = trainFeatures(1,1:numTrainPoints-60);
trainFeatures(5,61:numTrainPoints) = trainFeatures(2,1:numTrainPoints-60);
trainFeatures(6,61:numTrainPoints) = trainFeatures(3,1:numTrainPoints-60);

trainFeatures(7,151:numTrainPoints) = trainFeatures(1,1:numTrainPoints-150);
trainFeatures(8,151:numTrainPoints) = trainFeatures(2,1:numTrainPoints-150);
trainFeatures(9,151:numTrainPoints) = trainFeatures(3,1:numTrainPoints-150);

trainFeatures(10,301:numTrainPoints) = trainFeatures(1,1:numTrainPoints-300);
trainFeatures(11,301:numTrainPoints) = trainFeatures(2,1:numTrainPoints-300);
trainFeatures(12,301:numTrainPoints) = trainFeatures(3,1:numTrainPoints-300);

%------------------------------------------------------------------------------------

% Train on the first numTrainPoints
trainOutput = y(1:numTrainPoints);
theta = glmfit(trainFeatures', trainOutput,'normal')


[F, H, Q, R, initX, initV] = kl(y)

%--------------------------------------------------------------------------


% Test on the next 20000
testFeatures = zeros(numFeatures, numTestPoints);
testFeatures(1,:) = lecturer(numTrainPoints+1:numTrainPoints+numTestPoints,2)';
testFeatures(2,:) = saliency(numTrainPoints+1:numTrainPoints+numTestPoints,2)';
testFeatures(3,:) = saliency(numTrainPoints+1:numTrainPoints+numTestPoints,3)';

testFeatures(4,61:numTestPoints) = testFeatures(1,1:numTestPoints-60);
testFeatures(5,61:numTestPoints) = testFeatures(2,1:numTestPoints-60);
testFeatures(6,61:numTestPoints) = testFeatures(3,1:numTestPoints-60);

testFeatures(7,151:numTestPoints) = testFeatures(1,1:numTestPoints-150);
testFeatures(8,151:numTestPoints) = testFeatures(2,1:numTestPoints-150);
testFeatures(9,151:numTestPoints) = testFeatures(3,1:numTestPoints-150);

testFeatures(10,301:numTestPoints) = testFeatures(1,1:numTestPoints-300);
testFeatures(11,301:numTestPoints) = testFeatures(2,1:numTestPoints-300);
testFeatures(12,301:numTestPoints) = testFeatures(3,1:numTestPoints-300);

%-------------------------------------------------------------------------------------
%Smoothing filter

 testOutput = theta' * [ones(1,numTestPoints);testFeatures];
 testOutput = ks(testOutput,10000000000,10000000000);
 %testOutput = smooth(testOutput,500)';
 testOutput = testOutput(1,:);

%--------------------------------------------------------------------------

% Calculate the average error (in number of pixels)
errors = y(numTrainPoints+1:numTrainPoints+numTestPoints) - testOutput';

averageError = mean(errors);

% Plot both 
% t=1:numTestPoints;
% figure(1)
% plot(t,testOutput);
% legend('Test');
% print('graph7.png', '-dpng')

t=1:numTestPoints;

sq_err = sum(errors.^2)
figure(2)
plot(t,testOutput,t,y(numTrainPoints+1:numTrainPoints+numTestPoints));
title(['Errors: ' num2str(sq_err)]);
legend('Estimiated Traj','Actual');
print('graph4.png', '-dpng')




